home *** CD-ROM | disk | FTP | other *** search
/ Cream of the Crop 26 / Cream of the Crop 26.iso / os2 / octa209s.zip / octave-2.09 / scripts / control / lqr.m < prev    next >
Text File  |  1996-07-15  |  3KB  |  101 lines
  1. ## Copyright (C) 1996 John W. Eaton
  2. ##
  3. ## This file is part of Octave.
  4. ##
  5. ## Octave is free software; you can redistribute it and/or modify it
  6. ## under the terms of the GNU General Public License as published by
  7. ## the Free Software Foundation; either version 2, or (at your option)
  8. ## any later version.
  9. ##
  10. ## Octave is distributed in the hope that it will be useful, but
  11. ## WITHOUT ANY WARRANTY; without even the implied warranty of
  12. ## MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
  13. ## General Public License for more details.
  14. ##
  15. ## You should have received a copy of the GNU General Public License
  16. ## along with Octave; see the file COPYING.  If not, write to the Free
  17. ## Software Foundation, 59 Temple Place - Suite 330, Boston, MA
  18. ## 02111-1307, USA.
  19.  
  20. ## Usage: [k, p, e] = lqr (A, B, Q, R {,Z})
  21. ##
  22. ## Linear quadratic regulator design for the continuous time system
  23. ##
  24. ##   dx/dt = A x + B u
  25. ##
  26. ## to minimize the cost functional
  27. ##
  28. ##  J = int_0^\infty{ x' Q x + u' R u }             Z omitted
  29. ##
  30. ## or
  31. ##
  32. ##  J = int_0^\infty{ x' Q x + u' R u +2 x' Z u}        Z included
  33. ##
  34. ## Returns:
  35. ##
  36. ##   k = state feedback gain, (A - B K) is stable
  37. ##   p = solution of algebraic Riccati equation
  38. ##   e = closed loop poles of (A - B K)
  39.  
  40. ## Author: A. S. Hodel <scotte@eng.auburn.edu>
  41. ## Created: August 1993
  42. ## Adapted-By: jwe
  43.  
  44. function [k, p, e] = lqr (a, b, q, r, zz)
  45.  
  46.   if (nargin != 4 && nargin != 5)
  47.     error ("lqr: invalid number of arguments");
  48.   endif
  49.  
  50.   ## Check a.
  51.   if ((n = is_square (a)) == 0)
  52.     error ("lqr: requires 1st parameter(a) to be square");
  53.   endif
  54.  
  55.   ## Check b.
  56.   [n1, m] = size (b);
  57.   if (n1 != n)
  58.     error ("lqr: a,b not conformal");
  59.   endif
  60.  
  61.   ## Check q.
  62.  
  63.   if ((n1 = is_square (q)) == 0 || n1 != n)
  64.     error ("lqr: q must be square and conformal with a");
  65.   endif
  66.  
  67.   ## Check r.
  68.   if((m1 = is_square(r)) == 0 || m1 != m)
  69.     error ("lqr: r must be square and conformal with column dimension of b");
  70.   endif
  71.  
  72.   ## Check if n is there.
  73.   if (nargin == 5)
  74.     [n1, m1] = size (zz);
  75.     if (n1 != n || m1 != m)
  76.       error ("lqr: z must be identically dimensioned with b");
  77.     endif
  78.  
  79.     ## Incorporate cross term into a and q.
  80.  
  81.     ao = a - (b/r)*zz';
  82.     qo = q - (zz/r)*zz';
  83.   else
  84.     zz = zeros (n, m);
  85.     ao = a;
  86.     qo = q;
  87.   endif
  88.  
  89.   ## Check that q, (r) are symmetric, positive (semi)definite
  90.  
  91.   if (is_symmetric (q) && is_symmetric (r) ...
  92.       && all (eig (q) >= 0) && all (eig (r) > 0))
  93.     p = are (ao, (b/r)*b', qo);
  94.     k = r\(b'*p + zz');
  95.     e = eig (a - b*k);
  96.   else
  97.     error ("lqr: q (r) must be symmetric positive (semi) definite");
  98.   endif
  99.  
  100. endfunction
  101.